/**
 * @file message_queue.c
 * @brief 消息队列实现
 */

#include "action_camera.h"

/**
 * @brief 初始化消息队列
 * @param queue 消息队列指针
 * @param capacity 队列容量
 * @return 0成功，-1失败
 */
int message_queue_init(message_queue_t* queue, uint32_t capacity) {
    if (!queue || capacity == 0) {
        printf("[ERROR] message_queue_init: Invalid parameters\n");
        return -1;
    }
    
    // 清空结构体
    memset(queue, 0, sizeof(message_queue_t));
    
    queue->messages = malloc(sizeof(message_t) * capacity);
    if (!queue->messages) {
        printf("[ERROR] message_queue_init: Failed to allocate messages array\n");
        return -1;
    }
    
    // 清空消息数组
    memset(queue->messages, 0, sizeof(message_t) * capacity);
    
    queue->head = 0;
    queue->tail = 0;
    queue->capacity = capacity;
    queue->count = 0;
    
    printf("[DEBUG] Initializing mutex...\n");
    int mutex_ret = pthread_mutex_init(&queue->mutex, NULL);
    if (mutex_ret != 0) {
        printf("[ERROR] message_queue_init: Failed to init mutex: %d\n", mutex_ret);
        free(queue->messages);
        queue->messages = NULL;
        return -1;
    }
    
    printf("[DEBUG] Initializing not_empty condition...\n");
    int cond1_ret = pthread_cond_init(&queue->not_empty, NULL);
    if (cond1_ret != 0) {
        printf("[ERROR] message_queue_init: Failed to init not_empty condition: %d\n", cond1_ret);
        pthread_mutex_destroy(&queue->mutex);
        free(queue->messages);
        queue->messages = NULL;
        return -1;
    }
    
    printf("[DEBUG] Initializing not_full condition...\n");
    int cond2_ret = pthread_cond_init(&queue->not_full, NULL);
    if (cond2_ret != 0) {
        printf("[ERROR] message_queue_init: Failed to init not_full condition: %d\n", cond2_ret);
        pthread_cond_destroy(&queue->not_empty);
        pthread_mutex_destroy(&queue->mutex);
        free(queue->messages);
        queue->messages = NULL;
        return -1;
    }
    
    printf("[DEBUG] Message queue initialized successfully: capacity=%u, messages=%p\n", 
           capacity, (void*)queue->messages);
    
    return 0;
}

/**
 * @brief 发送消息到队列
 * @param queue 消息队列指针
 * @param msg 消息指针
 * @return 0成功，-1失败
 */
int message_queue_send(message_queue_t* queue, const message_t* msg) {
    if (!queue || !msg) {
        return -1;
    }
    
    pthread_mutex_lock(&queue->mutex);
    
    // 等待队列有空间
    while (queue->count >= queue->capacity) {
        pthread_cond_wait(&queue->not_full, &queue->mutex);
    }
    
    // 添加消息到队列
    queue->messages[queue->tail] = *msg;
    queue->tail = (queue->tail + 1) % queue->capacity;
    queue->count++;
    
    // 通知等待的接收者
    pthread_cond_signal(&queue->not_empty);
    
    pthread_mutex_unlock(&queue->mutex);
    
    return 0;
}

/**
 * @brief 从队列接收消息
 * @param queue 消息队列指针
 * @param msg 消息存储指针
 * @param timeout_ms 超时时间(毫秒)，0表示不等待
 * @return 0成功，-1超时或失败，-ETIMEDOUT表示超时
 */
int message_queue_receive(message_queue_t* queue, message_t* msg, uint32_t timeout_ms) {
    // 更严格的参数验证
    if (!queue || !msg) {
        printf("[ERROR] message_queue_receive: Invalid queue or message pointer\n");
        return -1;
    }
    
    // 验证队列结构完整性
    if (!queue->messages || queue->capacity == 0) {
        printf("[ERROR] message_queue_receive: Invalid queue structure (messages=%p, capacity=%u)\n",
               (void *)queue->messages, queue->capacity);
        return -1;
    }
    
    // 验证互斥锁是否有效
    printf("[DEBUG] Checking mutex validity before lock...\n");
    int mutex_test = pthread_mutex_trylock(&queue->mutex);
    if (mutex_test == 0) {
        printf("[DEBUG] Mutex test successful, unlocking...\n");
        pthread_mutex_unlock(&queue->mutex);
    } else if (mutex_test == EBUSY) {
        printf("[DEBUG] Mutex is busy (normal condition)\n");
    } else {
        printf("[ERROR] Mutex test failed with error: %d\n", mutex_test);
        return -1;
    }
    
    // 验证队列索引有效性
    if (queue->head >= queue->capacity || queue->tail >= queue->capacity || queue->count > queue->capacity) {
        printf("[ERROR] message_queue_receive: Invalid queue indices (head=%u, tail=%u, count=%u, capacity=%u)\n",
               queue->head, queue->tail, queue->count, queue->capacity);
        
        // 尝试修复队列索引
        printf("[DEBUG] Attempting to lock mutex for repair...\n");
        int lock_result = pthread_mutex_lock(&queue->mutex);
        if (lock_result != 0) {
            printf("[ERROR] Failed to lock mutex for repair: %d\n", lock_result);
            return -1;
        }
        printf("[DEBUG] Mutex locked successfully for repair\n");
        
        if (queue->count > queue->capacity) {
            queue->count = queue->capacity;
        }
        if (queue->head >= queue->capacity) {
            queue->head = 0;
        }
        if (queue->tail >= queue->capacity) {
            queue->tail = queue->count;
        }
        
        printf("[DEBUG] About to unlock mutex after repair...\n");
        int unlock_result = pthread_mutex_unlock(&queue->mutex);
        if (unlock_result != 0) {
            printf("[ERROR] Failed to unlock mutex after repair: %d\n", unlock_result);
            return -1;
        }
        printf("[DEBUG] Mutex unlocked successfully after repair\n");
        
        return -1;
    }
    
    printf("[DEBUG] About to lock main mutex...\n");
    int main_lock_result = pthread_mutex_lock(&queue->mutex);
    if (main_lock_result != 0) {
        printf("[ERROR] Failed to lock main mutex: %d\n", main_lock_result);
        return -1;
    }
    printf("[DEBUG] Main mutex locked successfully\n");
    
    // 如果队列为空且不等待
    if (queue->count == 0 && timeout_ms == 0) {
        printf("[DEBUG] Queue empty and no timeout, unlocking and returning\n");
        int unlock_result = pthread_mutex_unlock(&queue->mutex);
        if (unlock_result != 0) {
            printf("[ERROR] Failed to unlock mutex (empty queue): %d\n", unlock_result);
        }
        return -1;
    }
    
    // 等待队列非空
    while (queue->count == 0) {
        printf("[DEBUG] Queue is empty, waiting...\n");
        if (timeout_ms > 0) {
            struct timespec timeout;
            printf("[DEBUG] Getting current time for timeout calculation...\n");
            if (clock_gettime(CLOCK_REALTIME, &timeout) != 0) {
                printf("[ERROR] message_queue_receive: Failed to get current time\n");
                int unlock_result = pthread_mutex_unlock(&queue->mutex);
                if (unlock_result != 0) {
                    printf("[ERROR] Failed to unlock mutex (clock error): %d\n", unlock_result);
                }
                return -1;
            }
            
            printf("[DEBUG] Current time: sec=%ld, nsec=%ld\n", timeout.tv_sec, timeout.tv_nsec);
            
            // 计算超时时间 - 使用更安全的方法
            long timeout_nsec = (timeout_ms % 1000) * 1000000L;
            timeout.tv_sec += timeout_ms / 1000;
            timeout.tv_nsec += timeout_nsec;
            
            // 处理纳秒溢出
            if (timeout.tv_nsec >= 1000000000L) {
                timeout.tv_sec++;
                timeout.tv_nsec -= 1000000000L;
            }
            
            printf("[DEBUG] Calculated timeout: sec=%ld, nsec=%ld\n", timeout.tv_sec, timeout.tv_nsec);
            printf("[DEBUG] About to call pthread_cond_timedwait with timeout=%u ms\n", timeout_ms);
            
            // 验证条件变量有效性
            printf("[DEBUG] Verifying condition variable before timedwait...\n");
            
            int ret = pthread_cond_timedwait(&queue->not_empty, &queue->mutex, &timeout);
            printf("[DEBUG] pthread_cond_timedwait returned: %d\n", ret);
            if (ret == ETIMEDOUT) {
                printf("[DEBUG] Timeout occurred, unlocking and returning\n");
                int unlock_result = pthread_mutex_unlock(&queue->mutex);
                if (unlock_result != 0) {
                    printf("[ERROR] Failed to unlock mutex (timeout): %d\n", unlock_result);
                }
                return -ETIMEDOUT; // 返回具体的超时错误码
            } else if (ret != 0) {
                printf("[ERROR] message_queue_receive: pthread_cond_timedwait failed with code: %d\n", ret);
                int unlock_result = pthread_mutex_unlock(&queue->mutex);
                if (unlock_result != 0) {
                    printf("[ERROR] Failed to unlock mutex (cond_timedwait error): %d\n", unlock_result);
                }
                return -1;
            }
        } else {
            printf("[DEBUG] Calling pthread_cond_wait (no timeout)...\n");
            int ret = pthread_cond_wait(&queue->not_empty, &queue->mutex);
            printf("[DEBUG] pthread_cond_wait returned: %d\n", ret);
            if (ret != 0) {
                printf("[ERROR] message_queue_receive: pthread_cond_wait failed with code: %d\n", ret);
                int unlock_result = pthread_mutex_unlock(&queue->mutex);
                if (unlock_result != 0) {
                    printf("[ERROR] Failed to unlock mutex (cond_wait error): %d\n", unlock_result);
                }
                return -1;
            }
        }
        
        // 再次检查系统运行状态，避免在等待过程中系统已停止
        if (!atomic_load(&g_camera_system.sync.system_running)) {
            printf("[DEBUG] System not running, unlocking and returning\n");
            int unlock_result = pthread_mutex_unlock(&queue->mutex);
            if (unlock_result != 0) {
                printf("[ERROR] Failed to unlock mutex (system not running): %d\n", unlock_result);
            }
            return -1;
        }
        printf("[DEBUG] Wait completed, rechecking queue count: %u\n", queue->count);
    }
    
    // 再次验证队列状态，确保在等待过程中没有被破坏
    if (queue->count == 0 || !queue->messages || queue->head >= queue->capacity) {
        printf("[ERROR] message_queue_receive: Invalid queue state after waiting (count=%u, head=%u, capacity=%u)\n",
               queue->count, queue->head, queue->capacity);
        int unlock_result = pthread_mutex_unlock(&queue->mutex);
        if (unlock_result != 0) {
            printf("[ERROR] Failed to unlock mutex (invalid state): %d\n", unlock_result);
        }
        return -1;
    }
    
    printf("[DEBUG] About to copy message from queue\n");
    // 从队列取出消息
    *msg = queue->messages[queue->head];
    printf("[DEBUG] Message copied successfully\n");
    
    // 更新队列状态
    queue->head = (queue->head + 1) % queue->capacity;
    queue->count--;
    printf("[DEBUG] Queue state updated: head=%u, count=%u\n", queue->head, queue->count);
    
    // 确保count不为负数
    if (queue->count > queue->capacity) { // 检查溢出
        printf("[WARN] Queue count overflow detected, resetting to 0\n");
        queue->count = 0;
    }
    
    printf("[DEBUG] About to signal not_full condition\n");
    // 通知等待的发送者
    int signal_result = pthread_cond_signal(&queue->not_full);
    if (signal_result != 0) {
        printf("[ERROR] Failed to signal not_full condition: %d\n", signal_result);
    } else {
        printf("[DEBUG] not_full condition signaled successfully\n");
    }
    
    printf("[DEBUG] About to unlock final mutex...\n");
    int final_unlock_result = pthread_mutex_unlock(&queue->mutex);
    if (final_unlock_result != 0) {
        printf("[ERROR] Failed to unlock final mutex: %d\n", final_unlock_result);
        return -1;
    }
    printf("[DEBUG] Final mutex unlocked successfully\n");
    
    return 0;
}

/**
 * @brief 销毁消息队列
 * @param queue 消息队列指针
 */
void message_queue_destroy(message_queue_t* queue) {
    if (!queue) {
        return;
    }
    
    pthread_mutex_lock(&queue->mutex);
    
    SAFE_FREE(queue->messages);
    queue->head = 0;
    queue->tail = 0;
    queue->capacity = 0;
    queue->count = 0;
    
    pthread_mutex_unlock(&queue->mutex);
    
    pthread_cond_destroy(&queue->not_full);
    pthread_cond_destroy(&queue->not_empty);
    pthread_mutex_destroy(&queue->mutex);
}

/**
 * @brief 初始化帧缓冲池
 * @param pool 帧缓冲池指针
 * @param pool_size 缓冲池大小
 * @return 0成功，-1失败
 */
int frame_pool_init(frame_pool_t* pool, uint32_t pool_size) {
    if (!pool || pool_size == 0) {
        return -1;
    }
    
    pool->frames = malloc(sizeof(shared_frame_t) * pool_size);
    if (!pool->frames) {
        return -1;
    }
    
    pool->pool_size = pool_size;
    atomic_init(&pool->free_count, pool_size);
    pool->next_free = 0;
    
    if (pthread_mutex_init(&pool->pool_mutex, NULL) != 0) {
        free(pool->frames);
        return -1;
    }
    
    if (pthread_cond_init(&pool->frame_available, NULL) != 0) {
        pthread_mutex_destroy(&pool->pool_mutex);
        free(pool->frames);
        return -1;
    }
    
    // 初始化所有帧
    for (uint32_t i = 0; i < pool_size; i++) {
        pool->frames[i].frame_id = i;
        atomic_init(&pool->frames[i].ref_count, 0);
        pool->frames[i].size = 0;
    }
    
    return 0;
}

/**
 * @brief 从帧缓冲池获取帧
 * @param pool 帧缓冲池指针
 * @return 帧指针，失败返回NULL
 */
shared_frame_t* frame_pool_acquire(frame_pool_t* pool) {
    if (!pool) {
        return NULL;
    }
    
    pthread_mutex_lock(&pool->pool_mutex);
    
    // 等待有空闲帧
    while (atomic_load(&pool->free_count) == 0) {
        pthread_cond_wait(&pool->frame_available, &pool->pool_mutex);
    }
    
    // 查找空闲帧
    shared_frame_t* frame = NULL;
    for (uint32_t i = 0; i < pool->pool_size; i++) {
        uint32_t index = (pool->next_free + i) % pool->pool_size;
        if (atomic_load(&pool->frames[index].ref_count) == 0) {
            frame = &pool->frames[index];
            pool->next_free = (index + 1) % pool->pool_size;
            break;
        }
    }
    
    if (frame) {
        atomic_fetch_sub(&pool->free_count, 1);
        atomic_store(&frame->ref_count, 1);
        frame->size = 0;
        frame->timestamp = 0;
    }
    
    pthread_mutex_unlock(&pool->pool_mutex);
    
    return frame;
}

/**
 * @brief 释放帧到缓冲池
 * @param pool 帧缓冲池指针
 * @param frame 帧指针
 */
void frame_pool_release(frame_pool_t* pool, shared_frame_t* frame) {
    if (!pool || !frame) {
        return;
    }
    
    int old_ref_count = atomic_fetch_sub(&frame->ref_count, 1);
    
    if (old_ref_count <= 1) {
        // 最后一个引用，释放帧
        pthread_mutex_lock(&pool->pool_mutex);
        
        atomic_store(&frame->ref_count, 0);
        frame->size = 0;
        
        atomic_fetch_add(&pool->free_count, 1);
        pthread_cond_signal(&pool->frame_available);
        
        pthread_mutex_unlock(&pool->pool_mutex);
    }
}

/**
 * @brief 销毁帧缓冲池
 * @param pool 帧缓冲池指针
 */
void frame_pool_destroy(frame_pool_t* pool) {
    if (!pool) {
        return;
    }
    
    pthread_mutex_lock(&pool->pool_mutex);
    
    SAFE_FREE(pool->frames);
    pool->pool_size = 0;
    atomic_store(&pool->free_count, 0);
    pool->next_free = 0;
    
    pthread_mutex_unlock(&pool->pool_mutex);
    
    pthread_cond_destroy(&pool->frame_available);
    pthread_mutex_destroy(&pool->pool_mutex);
}